///@file Quad4.cpp
///@brief Implementation of the class representing a quadrangle element with 4 nodes
///@author Arnaud Duval
///@version 0.0
///@date 2009/11/03

#include "Quad4.h"

#include <cmath>

///@brief Constructor specifying the nodes
///@param n1 Pointer to the first node
///@param n2 Pointer to the second node
///@param n3 Pointer to the third node
///@param n4 Pointer to the fourth node
Quad4::Quad4(Node* n1, Node* n2, Node* n3, Node* n4)
{
    assert(n1->Dim() == n2->Dim());
    assert(n1->Dim() == n3->Dim());
    assert(n1->Dim() == n4->Dim());
    nodes.push_back(n1);
    nodes.push_back(n2);
    nodes.push_back(n3);
    nodes.push_back(n4);
    for(unsigned int i = 0 ; i < 4 ; i++)
    {
        gaussPoints[i].SetDimension(2u);
    }
    gaussPoints[0].SetRefCoord(1u, -1./sqrt(3.));
    gaussPoints[0].SetRefCoord(2u, -1./sqrt(3.));
    gaussPoints[1].SetRefCoord(1u, 1./sqrt(3.));
    gaussPoints[1].SetRefCoord(2u, -1./sqrt(3.));
    gaussPoints[2].SetRefCoord(1u, 1./sqrt(3.));
    gaussPoints[2].SetRefCoord(2u, 1./sqrt(3.));
    gaussPoints[3].SetRefCoord(1u, -1./sqrt(3.));
    gaussPoints[3].SetRefCoord(2u, 1./sqrt(3.));
}

///@brief Constructor with arguments
///@param n1 Pointer to the first node
///@param n2 Pointer to the second node
///@param n3 Pointer to the third node
///@param n4 Pointer to the fourth node
///@param theMaterial Pointer to the material from which the element is composed
///@param plop This parameter seems to be obsolete
Quad4::Quad4(Node* n1, Node* n2, Node* n3, Node* n4, Material* theMaterial, double plop)
{
    assert(n1->Dim() == n2->Dim());
    assert(n1->Dim() == n3->Dim());
    assert(n1->Dim() == n4->Dim());
    nodes.push_back(n1);
    nodes.push_back(n2);
    nodes.push_back(n3);
    nodes.push_back(n4);
    mat = theMaterial;
    for(unsigned int i = 0 ; i < 4 ; i++)
    {
        gaussPoints[i].SetMaterial(mat);
        gaussPoints[i].SetDimension(2u);
    }
    gaussPoints[0].SetRefCoord(1, -1./sqrt(3.));
    gaussPoints[0].SetRefCoord(2, -1./sqrt(3.));
    gaussPoints[1].SetRefCoord(1, 1./sqrt(3.));
    gaussPoints[1].SetRefCoord(2, -1./sqrt(3.));
    gaussPoints[2].SetRefCoord(1, 1./sqrt(3.));
    gaussPoints[2].SetRefCoord(2, 1./sqrt(3.));
    gaussPoints[3].SetRefCoord(1, -1./sqrt(3.));
    gaussPoints[3].SetRefCoord(2, 1./sqrt(3.));
}

///@brief Destructor
Quad4::~Quad4()
{
    for(std::list<Node*>::iterator i = nodes.begin(); i != nodes.end() ; i++)
    {
        *i = NULL;
    }
    mat = NULL;
}

///@brief Update specific values of element
void Quad4::Update()
{

}

///@brief compute nodal residual from nodal DOF increment
///@param Du Increment of nodal DOF
///@param Df nodal residuals
///@param K stiffness matrix
void Quad4::Equilibrate(const Vector& Du, Vector &Df, Matrix &K)
{
    //Prévoir récupération des variables aux points de Gauss
    ///TODO

    //Balancing the behaviour law at each Gauss point
    for(unsigned int i = 1 ; i <= 4 ; i++)
    {
        Vector dstrain = B(i) | Du;
        ///TO BE CONTINUED
    }
}

///@brief Compute nodal residual from nodal DOF increment with similarities to Abaqus UEL
///@param Df nodal residuals
///@param K stiffness matrix
///@param vars evolution variables
///@param u nodal DOF at beginning of increment
///@param Du Increment of nodal DOF
void Quad4::UEL(Vector& Df, Matrix& K, Vector& vars, const Vector& u, const Vector& Du)
{
    std::cout << "UEL Quad4\n";
    //Prévoir récupération des variables aux points de Gauss
    ///TODO

    //Balancing the behaviour law at each Gauss point
    Vector stress[4];
    Matrix ddsdde[4];
    for(unsigned int i = 1 ; i <= 4 ; i++)
    {
        Vector dstrain = B(i) | Du;
        Vector strain = B(i) | u;


        bool OK = mat->UMAT(strain, dstrain, stress[i-1], ddsdde[i-1]);
    }

    //Initializing K to zero
    for(unsigned int i = 1 ; i <= 8 ; i++)
    {
        Df[i] = 0.;
        for(unsigned int j = 1 ; j <= 8 ; j++)
        {
            K(i,j) = 0.;
        }
    }

    //Adding the contribution of each Gauss point
    for(unsigned int i = 1 ; i <= 4 ; i++)
    {
        Matrix Bgauss = B(i);
        Matrix Jtrans = Jacobian(i);
        double detJtrans = Jtrans.Det();

        ///@warning We have defined an arbitrary width
        double width = 1.;
        K = K + width*((Bgauss.Transpose())|ddsdde[i-1]|Bgauss)*detJtrans;
        Df = Df + width*((Bgauss.Transpose())|stress[i-1])*detJtrans;
    }
    
}

///@brief Compute displacements interpolation matrix at a given intgration point
///@param gaussPoint Number of the Gauss point considered (starts at 1)
Matrix Quad4::N(unsigned int gaussPoint)
{
    double xi, eta;
    GetGaussCoords(gaussPoint, xi, eta);
    double N1 = (1. - xi)*(1. - eta)/4.;
    double N2 = (1. + xi)*(1. - eta)/4.;
    double N3 = (1. + xi)*(1. + eta)/4.;
    double N4 = (1. - xi)*(1. + eta)/4.;

    Matrix res(2u, 3u);
    
    res(1,1) = N1;
    res(1,2) = 0.;
    res(1,3) = N2;
    res(1,4) = 0.;
    res(1,5) = N3;
    res(1,6) = 0.;
    res(1,7) = N4;
    res(1,8) = 0.;

    res(2,1) = 0.;
    res(2,2) = N1;
    res(2,3) = 0.;
    res(2,4) = N2;
    res(2,5) = 0.;
    res(2,6) = N3;
    res(2,7) = 0.;
    res(2,8) = N4;

	return res;
}

///@brief Compute strain matrix operator at a given integration point
///@param gaussPoint Number of the Gauss point considered (starts at 1)
Matrix Quad4::B(unsigned int gaussPoint)
{
    double xi, eta;
    GetGaussCoords(gaussPoint, xi, eta);

    //Computing the strain operator
    Matrix Mdef(3u,4u);
    Mdef(1,1) = 1.;
    Mdef(1,2) = 0.;
    Mdef(1,3) = 0.;
    Mdef(1,4) = 0.;
    Mdef(2,1) = 0.;
    Mdef(2,2) = 0.;
    Mdef(2,3) = 0.;
    Mdef(2,4) = 1.;
    Mdef(3,1) = 0.;
    Mdef(3,2) = 1.;
    Mdef(3,3) = 1.;
    Mdef(3,4) = 1.;

    double dN1dxi = (eta - 1.)/4.;
    double dN1deta = (xi - 1.)/4.;
    double dN2dxi = -(eta - 1.)/4.;
    double dN2deta = -(xi + 1.)/4.;
    double dN3dxi = (eta +1.)/4.;
    double dN3deta = (xi +1.)/4.;
    double dN4dxi = -(eta + 1.)/4.;
    double dN4deta = -(xi - 1.)/4.;


    //Computing the displacement gradient operator
    Matrix Mgrad(4u,8u);
    
    Mgrad(1,1) = dN1dxi;
    Mgrad(1,2) = 0.;
    Mgrad(1,3) = dN2dxi;
    Mgrad(1,4) = 0.;
    Mgrad(1,5) = dN3dxi;
    Mgrad(1,6) = 0.;
    Mgrad(1,7) = dN4dxi;
    Mgrad(1,8) = 0.;

    Mgrad(2,1) = dN1deta;
    Mgrad(2,2) = 0.;
    Mgrad(2,3) = dN2deta;
    Mgrad(2,4) = 0.;
    Mgrad(2,5) = dN3deta;
    Mgrad(2,6) = 0.;
    Mgrad(2,7) = dN4deta;
    Mgrad(2,8) = 0.;

    Mgrad(3,1) = 0.;
    Mgrad(3,2) = dN1dxi;
    Mgrad(3,3) = 0.;
    Mgrad(3,4) = dN2dxi;
    Mgrad(3,5) = 0.;
    Mgrad(3,6) = dN3dxi;
    Mgrad(3,7) = 0.;
    Mgrad(3,8) = dN4dxi;

    Mgrad(4,1) = 0.;
    Mgrad(4,2) = dN1deta;
    Mgrad(4,3) = 0.;
    Mgrad(4,4) = dN2deta;
    Mgrad(4,5) = 0.;
    Mgrad(4,6) = dN3deta;
    Mgrad(4,7) = 0.;
    Mgrad(4,8) = dN4deta;

    //Computing the transformation Jacobian
    //Part 1 : nodes coordinates
    Matrix nodeCoords(4u, 2u);
    unsigned int i = 1;
    ///@warning The following "for" loop MUST be checked to ensure all nodes are taken into account
    for(std::list<Node*>::iterator lit = nodes.begin() ; lit != nodes.end() ; ++lit)
    {
        ///@warning This affectation could be improved with a "for" loop
        nodeCoords(i, 1) = (*lit)->GetCoordinate(1);
        nodeCoords(i, 2) = (*lit)->GetCoordinate(2);
        i++;
    }
    //Part 2 : gradient of ointerpolation functions
    Matrix gradInt(2u, 4u);
    ///@warning Maybe a single Matrix could be used instead of Matrices gradInt and Mgrad
    gradInt(1,1) = dN1dxi;
    gradInt(2,1) = dN1deta;
    gradInt(1,2) = dN2dxi;
    gradInt(2,2) = dN2deta;
    gradInt(1,3) = dN3dxi;
    gradInt(2,3) = dN3deta;
    gradInt(1,4) = dN4dxi;
    gradInt(2,4) = dN4deta;
    //Part 3 : computing the jacobian matrix
    Matrix J = gradInt|nodeCoords;
    
    //Building the transformation matrix
    Matrix Jinv = J.Invert();
    Matrix Mtrans(4u, 4u);
    ///@warning We must be sure that all values in Mtrans are 0
    Mtrans.AddSubMatrix(Jinv, 1, 1);
    Mtrans.AddSubMatrix(Jinv, 3, 3);

    return (Mdef|Mtrans|Mgrad);
}

///@brief Compute transformation Jacobian matrix at a given integration point
///@param gaussPoint Number of the Gauss point considered (starts at 1)
Matrix Quad4::Jacobian(unsigned int gaussPoint)
{
    double xi, eta;
    GetGaussCoords(gaussPoint, xi, eta);

    double dN1dxi = (eta - 1.)/4.;
    double dN1deta = (xi - 1.)/4.;
    double dN2dxi = -(eta - 1.)/4.;
    double dN2deta = -(xi + 1.)/4.;
    double dN3dxi = (eta +1.)/4.;
    double dN3deta = (xi +1.)/4.;
    double dN4dxi = -(eta + 1.)/4.;
    double dN4deta = -(xi - 1.)/4.;
    
    //Computing the transformation Jacobian
    //Part 1 : nodes coordinates
    Matrix nodeCoords(4u, 2u);
    unsigned int i = 1;
    ///@warning The following "for" loop MUST be checked to ensure all nodes are taken into account
    for(std::list<Node*>::iterator lit = nodes.begin() ; lit != nodes.end() ; ++lit)
    {
        ///@warning This affectation could be improved with a "for" loop
        nodeCoords(i, 1) = (*lit)->GetCoordinate(1);
        nodeCoords(i, 2) = (*lit)->GetCoordinate(2);
        i++;
    }
    //Part 2 : gradient of ointerpolation functions
    Matrix gradInt(2u, 4u);
    ///@warning Maybe a single Matrix could be used instead of Matrices gradInt and Mgrad
    gradInt(1,1) = dN1dxi;
    gradInt(2,1) = dN1deta;
    gradInt(1,2) = dN2dxi;
    gradInt(2,2) = dN2deta;
    gradInt(1,3) = dN3dxi;
    gradInt(2,3) = dN3deta;
    gradInt(1,4) = dN4dxi;
    gradInt(2,4) = dN4deta;
    //Part 3 : computing the jacobian matrix

    return gradInt|nodeCoords;
}

///@brief Return coordinates of Gauss point
///@param gaussPoint Number of the Gauss point considered (starts at 1)
void Quad4::GetGaussCoords(unsigned int gaussPoint, double& xi, double& eta)
{
    assert((gaussPoint >= 1)&&(gaussPoint <= 4));
    xi = gaussPoints[gaussPoint-1].GetRefCoord(1);
    eta = gaussPoints[gaussPoint-1].GetRefCoord(2);
}
